package com.controller;

import com.common.api.ApiResult;
import com.dto.FlyCircleParam;
import com.dto.FlyLineParam;
import com.dto.FlyRectangleParam;
import com.entity.MyHomePosition;
import com.service.DroneService;
import com.service.ISecurityrestrictionService;
import io.dronefleet.mavlink.common.HomePosition;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.web.bind.annotation.*;

import java.io.IOException;
import java.util.HashMap;
import java.util.Map;

import com.utils.TaskQueueManager;

@RestController
@RequestMapping("/drone")
public class DroneController {
    @Autowired
    private DroneService droneService;

    @Autowired
    private ISecurityrestrictionService iSecurityrestrictionService;

    @PostMapping ("/setAutoTakeoffMode")
    public ApiResult setAutoTakeOff() throws IOException {
       droneService.setAutoTakeoffMode();
       return ApiResult.success("设置自动起飞模式指令已发送");
    }

    @PostMapping ("/sendLandCommand")
    public ApiResult sendLandCommand() throws IOException {
        droneService.sendLandCommand();
        return ApiResult.success("降落指令已发送");
    }

    @Autowired
    private TaskQueueManager taskQueueManager; // 注入任务队列管理器

    @PostMapping("/flyRectangleByP2P")
    public ApiResult sendFlyRectangleByP2PCmd(@RequestBody FlyRectangleParam flyRectangleParam) {
        // 安全防护（保持原有逻辑）
        double width = Math.abs(flyRectangleParam.getCurrent_x() - flyRectangleParam.getEndX());
        double height = Math.abs(flyRectangleParam.getCurrent_y() - flyRectangleParam.getEndY());
        double distance = 2 * (width + height);
        double speed = distance / flyRectangleParam.getCycle_s();
        if (flyRectangleParam.getFlightAltitude() > iSecurityrestrictionService.getById(1).getAltitude()) {
            flyRectangleParam.setFlightAltitude(iSecurityrestrictionService.getById(1).getAltitude().floatValue());
        }
        if (speed > iSecurityrestrictionService.getById(1).getSpeed()) {
            flyRectangleParam.setCycle_s((float) (distance / iSecurityrestrictionService.getById(1).getSpeed()));
        }

        // 添加任务到队列，而非直接执行
        taskQueueManager.addRectangleTask(flyRectangleParam);
        System.out.println("任务列表数量:"+taskQueueManager.getPendingTaskCount());

        return ApiResult.success(String.format(
                "矩形飞行任务已添加到队列，当前待执行任务数: %d",
                taskQueueManager.getPendingTaskCount()
        ));
    }

    @PostMapping("/flyCircle")
    public ApiResult sendFlyCirlceCmd(@RequestBody FlyCircleParam flyCircleParam) {
        // 安全防护（保持原有逻辑）
        double distance = 2 * Math.PI * flyCircleParam.getRadius();
        double speed = distance / flyCircleParam.getCycle_s();
        if (flyCircleParam.getFlightAltitude() > iSecurityrestrictionService.getById(1).getAltitude()) {
            flyCircleParam.setFlightAltitude(iSecurityrestrictionService.getById(1).getAltitude().floatValue());
        }
        if (speed > iSecurityrestrictionService.getById(1).getSpeed()) {
            flyCircleParam.setCycle_s((float) (distance / iSecurityrestrictionService.getById(1).getSpeed()));
        }

        // 添加任务到队列
        taskQueueManager.addCircleTask(flyCircleParam);
        System.out.println("任务列表数量:"+taskQueueManager.getPendingTaskCount());


        return ApiResult.success(String.format(
                "圆形飞行任务已添加到队列，当前待执行任务数: %d",
                taskQueueManager.getPendingTaskCount()
        ));
    }

    @PostMapping("/flyLine")
    public ApiResult sendFlyLineCmd(@RequestBody FlyLineParam flyLineParam) {
        // 安全防护（保持原有逻辑）
        float deltaX = flyLineParam.getStartX() - flyLineParam.getEndX();
        float deltaY = flyLineParam.getStartY() - flyLineParam.getEndY();
        double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
        double speed = distance / flyLineParam.getDuration_s();
        if (flyLineParam.getFlightAltitude() > iSecurityrestrictionService.getById(1).getAltitude()) {
            flyLineParam.setFlightAltitude( iSecurityrestrictionService.getById(1).getAltitude().floatValue());
        }
        if (speed > iSecurityrestrictionService.getById(1).getSpeed()) {
            flyLineParam.setDuration_s((float) (distance / iSecurityrestrictionService.getById(1).getSpeed()));
        }

        // 添加任务到队列
        taskQueueManager.addLineTask(flyLineParam);
        System.out.println("任务列表数量:"+taskQueueManager.getPendingTaskCount());

        return ApiResult.success(String.format(
                "直线飞行任务已添加到队列，当前待执行任务数: %d",
                taskQueueManager.getPendingTaskCount()
        ));
    }

    // 新增接口：查询待执行任务数
    @GetMapping("/pendingTasks")
    public ApiResult getPendingTaskCount() {
        return ApiResult.success(taskQueueManager.getPendingTaskCount());
    }

    /**
     * 暂停任务队列
     */
    @PostMapping("/queue/pause")
    public ApiResult pauseTaskQueue() {
        taskQueueManager.pause();
        return ApiResult.success("任务队列已暂停");
    }

    /**
     * 恢复任务队列
     */
    @PostMapping("/queue/resume")
    public ApiResult resumeTaskQueue() {
        taskQueueManager.resume();
        return ApiResult.success("任务队列已恢复");
    }//当前待执行任务数

    /**
     * 获取队列状态
     */
    @GetMapping("/queue/status")
    public ApiResult getQueueStatus() {
        Map<String, Object> result = new HashMap<>();
        result.put("isPaused", taskQueueManager.isPaused());
        result.put("pendingTasks", taskQueueManager.getPendingTaskCount());
        return ApiResult.success(result);
    }

    /**
     * 获取完整任务队列信息
     */
    @GetMapping("/queue/tasks")
    public ApiResult getTaskQueue() {
        return ApiResult.success(taskQueueManager.getAllTasks());
    }

//    @PostMapping("/flyRectangleByP2P")
//    public ApiResult sendFlyRectangleByP2PCmd(@RequestBody FlyRectangleParam flyRectangleParam) throws IOException, InterruptedException {
//        double width = Math.abs(flyRectangleParam.getCurrent_x() - flyRectangleParam.getEndX());
//        double height = Math.abs(flyRectangleParam.getCurrent_y() - flyRectangleParam.getEndY());
//        double distance = 2 * (width + height);
//        double speed = distance/ flyRectangleParam.getCycle_s();
//        //安全防护
//        if(flyRectangleParam.getFlightAltitude()>iSecurityrestrictionService.getById(1).getAltitude())
//            flyRectangleParam.setFlightAltitude((iSecurityrestrictionService.getById(1).getAltitude()).floatValue());
//        if(speed>iSecurityrestrictionService.getById(1).getSpeed())
//            flyRectangleParam.setCycle_s((float) (distance/(iSecurityrestrictionService.getById(1).getSpeed())));
//        droneService.flyRectangleByP2P(
//                flyRectangleParam.getFlightAltitude(),
////                (float)1.5,
//                flyRectangleParam.getEndX(), flyRectangleParam.getEndY(),
//                flyRectangleParam.getCurrent_x(), flyRectangleParam.getCurrent_y(),
//                flyRectangleParam.isClockwise(),
//                flyRectangleParam.getCycle_s()
////                10
//        );
//        return ApiResult.success("执行矩形轨迹飞行指令(P2P)已完成");
//    }
//
//    @PostMapping("/flyCircle")
//    public ApiResult sendFlyCirlceCmd(@RequestBody FlyCircleParam flyCircleParam) throws IOException, InterruptedException {
//        double distance = 2 * Math.PI * flyCircleParam.getRadius();
//        double speed = distance/ flyCircleParam.getCycle_s();
//        //安全防护
//        if(flyCircleParam.getFlightAltitude()>iSecurityrestrictionService.getById(1).getAltitude())
//            flyCircleParam.setFlightAltitude(iSecurityrestrictionService.getById(1).getAltitude().floatValue());
//        if(speed>iSecurityrestrictionService.getById(1).getSpeed())
//            flyCircleParam.setCycle_s((float) (distance/iSecurityrestrictionService.getById(1).getSpeed()));
////        System.out.println("----------顺序："+flyCircleParam.isUuu());
//        droneService.sendflyCircleCmdOnOffBorad(flyCircleParam.getRadius(),
//                flyCircleParam.isClockwise(), flyCircleParam.getCurrent_x(),
//                flyCircleParam.getCurrent_y(),
////                (float)10,
//                flyCircleParam.getFlightAltitude(),
//                flyCircleParam.getCycle_s(),
////                20,
//                (flyCircleParam.isUuu()));
//        return ApiResult.success("执行飞行圆形轨迹指令已完成");
//    }
//
//    @PostMapping("/flyLine")
//    public ApiResult sendFlyLineCmd(@RequestBody FlyLineParam flyLineParam) throws IOException, InterruptedException {
//        float deltaX = flyLineParam.getStartX() - flyLineParam.getEndX();
//        float deltaY = flyLineParam.getStartY() - flyLineParam.getEndY();
//        double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
//        double speed = distance/ flyLineParam.getDuration_s();
//        //安全防护
//        if(flyLineParam.getFlightAltitude()>iSecurityrestrictionService.getById(1).getAltitude())
//            flyLineParam.setFlightAltitude(iSecurityrestrictionService.getById(1).getAltitude().floatValue());
//        if(speed>iSecurityrestrictionService.getById(1).getSpeed())
//            flyLineParam.setDuration_s((float) (distance/iSecurityrestrictionService.getById(1).getSpeed()));
//        droneService.flyPointToPoint(flyLineParam.getStartX(),flyLineParam.getStartY(),
//                flyLineParam.getEndX(), flyLineParam.getEndY(),
//                flyLineParam.getFlightAltitude(),
//                flyLineParam.getDuration_s()
////                (float)5,
////                20
//        );
//        return ApiResult.success("执行飞行直线轨迹指令已完成");
//    }

    @PostMapping ("/return")
    public ApiResult setAutoReturnMode() throws IOException {
        droneService.setAutoReturnMode();
        return ApiResult.success("自动返航指令已完成");
    }

    @PostMapping("/setAutoLandMode")
    public ApiResult setAutoLandMode() throws IOException {
        droneService.setAutoLandMode();
        return ApiResult.success("设置自动降落模式指令已完成");
    }

    @PostMapping("/getHomePosition")
    public ApiResult getHomePosition() throws IOException, InterruptedException {
        HomePosition homePosition = droneService.requestHomePosition();
        MyHomePosition myHomePosition = new MyHomePosition(homePosition.latitude(),homePosition.longitude(),homePosition.altitude(),homePosition.x(),homePosition.y(),homePosition.z());
        return ApiResult.success(myHomePosition);
    }

    @GetMapping ("/test")
    public ApiResult test() throws IOException, InterruptedException {
        HomePosition homePosition = droneService.requestHomePosition();
        MyHomePosition myHomePosition = new MyHomePosition(homePosition.latitude(),homePosition.longitude(),homePosition.altitude(),homePosition.x(),homePosition.y(),homePosition.z());
        return ApiResult.success(myHomePosition);
    }

    @GetMapping ("/getFlightRealDataListByTimeId")
    public ApiResult getFlightRealDataListByTimeId (@RequestParam String timeId) {
        return ApiResult.success(droneService.getFlightRealDataListByTimeId(timeId));
    }

    @GetMapping ("/getAllRecordsTimeId")
    public ApiResult getAllRecordsTimeId () {
        return ApiResult.success(droneService.getAllTimeId());
    }

    @PostMapping("/disarm")
    public ApiResult disArm() throws IOException {
        droneService.disArm();
        return ApiResult.success("上锁指令已发送");
    }
}